; tree search 

(defconstant FAIL nil "no states")

(defun tree-search (states goal-p successors combiner)
  (dbg :search "~&;; Search ~a" states)
  (cond ((null states) fail)
	((funcall goal-p (first states)) (first states))
	(t (tree-search 
	    (funcall combiner
		     (funcall successors (first states))
		     (rest states))
	    goal-p successors combiner))))


(defun binary-tree (x) (list (* 2 x) (+ 1 (* 2 x))))

(defun depth-first-search (start goal-p successors)
  (tree-search (list start) goal-p successors #'append))

(defun is (value) (lambda(x) (eql value x)))

(defun prepend (x y) (append y x))

(defun breadth-first-search (start goal-p successors)
  (tree-search (list start) goal-p successors #'prepend))


(defun finite-binary-tree (n)
  #'(lambda(x)
      (remove-if #'(lambda(y) (> y n))
		 (binary-tree x))))


(defun diff (num)
  #'(lambda(x) (abs (- x num))))

(defun sorter (cost-fn)
  #'(lambda(new old)
      (sort (append new old) #'< :key cost-fn)))

(defun best-first-search (start goal-p successors cost-fn)
  (tree-search (list start) goal-p successors (sorter cost-fn)))

(defun beam-search (start goal-p successors cost-fn beam-width)
  (tree-search (list start) goal-p successors
	#'(lambda(old new)
	    (let ((sorted (funcall (sorter cost-fn) old new)))
	      (if (> beam-width (length sorted))
		  sorted
		  (subseq sorted 0 beam-width))))))


(defun price-is-right (price)
  #'(lambda(x) (if (> x price)
		   most-positive-fixnum
		   (- price x))))


(defstruct (city (:type list)) name long lat)

(defparameter *cities*
  '((Atlanta      84.23 33.45) (Los-Angeles   118.15 34.03)
    (Boston       71.05 42.21) (Memphis        90.03 35.09)  
    (Chicago      87.37 41.50) (New-York       73.58 40.47) 
    (Denver      105.00 39.45) (Oklahoma-City  97.28 35.26)
    (Eugene      123.05 44.03) (Pittsburgh     79.57 40.27) 
    (Flagstaff   111.41 35.13) (Quebec         71.11 46.49)
    (Grand-Jct   108.37 39.05) (Reno          119.49 39.30)
    (Houston     105.00 34.00) (San-Francisco 122.26 37.47)
    (Indianapolis 86.10 39.46) (Tampa          82.27 27.57)
    (Jacksonville 81.40 30.22) (Victoria      123.21 48.25)
    (Kansas-City  94.35 39.06) (Wilmington     77.57 34.14)))

(defun distance (p1 p2)
  (sqrt (reduce #'+ (mapcar #'(lambda(a b) (expt (- a b) 2))
	  p1 p2))))

(defconstant earth-diameter 12765.0)

(defun air-distance (city1 city2)
  (let ((d (distance (xyz-coords city1) (xyz-coords city2))))
    (* earth-diameter (asin (/ d 2)))))


(defun xyz-coords (city)
  (let ((psi (deg-radians (city-lat city)))
	(phi (deg-radians (city-long city))))
    (list (* (cos psi) (cos phi))
	  (* (cos psi) (sin phi))
	  (sin psi))))

(defun deg-radians (deg)
  (* (+ (truncate deg) (* (rem deg 1) 100/60)) pi 1/180))

(defun city (name)
  (assoc name *cities*))


(defun neighbors (city)
  (remove-if-not #'(lambda(c)
		     (and (not (eq c city))
			  (< (air-distance c city) 1000.0)))
		 *cities*))


(defun trip (start dest)
  (beam-search start (is dest) #'neighbors
	       #'(lambda(c) (air-distance c dest))
	       1))



(defstruct (path (:print-function print-path))
  state (previous nil) (cost-so-far 0) (total-cost 0))

(defun trip2 (start dest &optional (beam-width 1))
  (beam-search (make-path :state start)
	       (is-p dest :key #'path-state)
	       (path-saver #'neighbors #'air-distance #'(lambda(c)(air-distance c dest)))
	       #'path-total-cost
	       beam-width))


(defun is-p (value &key (key #'identity) (test #'eql))
  #'(lambda(path) (funcall test value (funcall key path))))

(defun path-saver (successors cost-fn cost-left-fn)
  #'(lambda(old-path)
      (let ((old-state (path-state old-path)))
	(mapcar
	 #'(lambda(new-state)
	     (let ((old-cost
		    (+ (path-cost-so-far old-path)
		       (funcall cost-fn old-state new-state))))
	       (make-path
		:state new-state
		:previous old-path
		:cost-so-far old-cost
		:total-cost (+ old-cost (funcall cost-left-fn new-state)))))
	 (funcall successors old-state)))))


(defun print-path (path &optional (stream t) depth)
  (declare (ignore depth))
  (format stream "#<Path to ~a cost ~,1f>"
	  (path-state path) (path-total-cost path)))

(defun show-city-path (path &optional (stream t))
  (format stream "#<Path ~,1f km: ~{~:(~a~)~^ - ~}>"
	  (path-total-cost path)
	  (reverse (map-path #'city-name path)))
  (values))


(defun map-path (fn path)
  (if (null path)
      nil
      (cons (funcall fn (path-state path))
	    (map-path fn (path-previous path)))))


(defun iter-wide-search (start goal-p successors cost-fn &key (width 1) (max 100))
  (dbg :search "; Width: ~d" width)
  (unless (> width max)
    (or (beam-search start goal-p successors cost-fn width)
	(iter-wide-search start goal-p successors cost-fn :width (+ width 1) :max max))))